/*
Copyright (C) 2021-2025 Casa Xu (also Zhiyan Xu) from HIT

This program is free software; you can redistribute it and/or
modify it under the terms of the GNU General Public License
as published by the Free Software Foundation; either version 2
of the License, or (at your option) any later version.

This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
GNU General Public License for more details.

You should have received a copy of the GNU General Public License
along with this program; if not, write to the Free Software
Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
*/

/*
 * LOS_Flywheel.c
 *
 *  Created on: 2022年12月14日
 *      Author: 64435
 */

#include "LOS_Flywheel.h"

LOS_FWSTA_t STA_FW[4]={{0,0,0,0,0,0,0,0,0,0,0},{0,0,0,0,0,0,0,0,0,0,0},{0,0,0,0,0,0,0,0,0,0,0},{0,0,0,0,0,0,0,0,0,0,0}};  //飞轮XYZS状态,对应0123

//暂存飞轮遥测包
uint8_t TMPACK_FWX[26];
uint8_t TMPACK_FWY[26];
uint8_t TMPACK_FWZ[26];
uint8_t TMPACK_FWS[26];

void LOS_FW_GetState(uint8_t FW_NUM)
{
    LOS_SNP_t SNP_CMD;                  //首部配置结构体
    uint8_t FP_CMD[8];                  //首部内容
    SNP_CMD.SNP_dest = nID_WheelX + FW_NUM;     //飞轮XYZS
    SNP_CMD.SNP_source = 0xF0;          //OBC发送
    SNP_CMD.SNP_rec = 0x00;             //内容识别码：0
    SNP_CMD.SNP_len = 8+8;              //长度（包含首部）
    SNP_CMD.SNP_tran = 0x01;            //0001B,信息传递服务
    LOS_CAN_FirstPart(&SNP_CMD,FP_CMD);
    LOS_CAN_Transmit(LOS_CAN_NUM1, LOS_CAN_IDGen(ACK_NoNeed, 0XF0, nID_WheelX + FW_NUM, 0X00), FP_CMD, 8);
    FP_CMD[0]=0X0D;
    FP_CMD[1]=nID_WheelX+FW_NUM;
    FP_CMD[2]=0XDD;
    FP_CMD[3]=0X00;
    FP_CMD[4]=0X00;
    FP_CMD[5]=0X00;
    FP_CMD[6]=0X00;
    FP_CMD[7]=FP_CMD[0]+FP_CMD[1]+FP_CMD[2]+FP_CMD[3]+FP_CMD[4]+FP_CMD[5]+FP_CMD[6];
    LOS_CAN_Transmit(LOS_CAN_NUM1, LOS_CAN_IDGen(ACK_NoNeed, 0XF0, nID_WheelX + FW_NUM, 0XFF), FP_CMD, 8);
    STA_FW[FW_NUM].HL_TXF = 1;
}

//动力学测试专用
void LOS_FW_GetState_Dyn(uint8_t FW_NUM)
{
    LOS_SNP_t SNP_CMD;                  //首部配置结构体
    uint8_t FP_CMD[8];                  //首部内容
    SNP_CMD.SNP_dest = 0xC1 + FW_NUM;     //飞轮XYZS
    SNP_CMD.SNP_source = 0xF0;          //OBC发送
    SNP_CMD.SNP_rec = 0x00;             //内容识别码：0
    SNP_CMD.SNP_len = 8+8;              //长度（包含首部）
    SNP_CMD.SNP_tran = 0x01;            //0001B,信息传递服务
    LOS_CAN_FirstPart(&SNP_CMD,FP_CMD);
    LOS_CAN_Transmit(LOS_CAN_NUM1, LOS_CAN_IDGen(ACK_NoNeed, 0XF0, 0xC1 + FW_NUM, 0X00), FP_CMD, 8);
    FP_CMD[0]=0X0D;
    FP_CMD[1]=0xC1+FW_NUM;
    FP_CMD[2]=0XDD;
    FP_CMD[3]=0X00;
    FP_CMD[4]=0X00;
    FP_CMD[5]=0X00;
    FP_CMD[6]=0X00;
    FP_CMD[7]=FP_CMD[0]+FP_CMD[1]+FP_CMD[2]+FP_CMD[3]+FP_CMD[4]+FP_CMD[5]+FP_CMD[6];
    LOS_CAN_Transmit(LOS_CAN_NUM1, LOS_CAN_IDGen(ACK_NoNeed, 0XF0, 0xC1 + FW_NUM, 0XFF), FP_CMD, 8);
    STA_FW[FW_NUM].HL_TXF = 1;
}



void LOS_FW_SetCurrent(uint8_t FW_NUM, float cur)
{
    LOS_SNP_t SNP_CMD;                  //首部配置结构体
    uint8_t FP_CMD[8];                  //首部内容
    SNP_CMD.SNP_dest = nID_WheelX + FW_NUM;     //飞轮XYZS
    SNP_CMD.SNP_source = 0xF0;          //OBC发送
    SNP_CMD.SNP_rec = 0x00;             //内容识别码：0
    SNP_CMD.SNP_len = 8+8;              //长度（包含首部）
    SNP_CMD.SNP_tran = 0x01;            //0001B,信息传递服务
    LOS_CAN_FirstPart(&SNP_CMD,FP_CMD);
    LOS_CAN_Transmit(LOS_CAN_NUM1, LOS_CAN_IDGen(ACK_NoNeed, 0XF0, nID_WheelX + FW_NUM, 0X00), FP_CMD, 8);

    if(cur>340)
    {
        cur = 340;  //电流最大设置为340mA
    }

    if(cur<-340)
    {
        cur = -340;
    }

    FP_CMD[0]=0X01;
    FP_CMD[1]=nID_WheelX+FW_NUM;
    FP_CMD[2]=0XD1;
    memcpy(&FP_CMD[3],&cur,4);
    FP_CMD[7]=FP_CMD[0]+FP_CMD[1]+FP_CMD[2]+FP_CMD[3]+FP_CMD[4]+FP_CMD[5]+FP_CMD[6];
    LOS_CAN_Transmit(LOS_CAN_NUM1, LOS_CAN_IDGen(ACK_NoNeed, 0XF0, nID_WheelX + FW_NUM, 0XFF), FP_CMD, 8);
}

void LOS_FW_SetAngvel(uint8_t FW_NUM, float angvel)
{
    LOS_SNP_t SNP_CMD;                  //首部配置结构体
    uint8_t FP_CMD[8];                  //首部内容
    SNP_CMD.SNP_dest = nID_WheelX + FW_NUM;     //飞轮XYZS
    SNP_CMD.SNP_source = 0xF0;          //OBC发送
    SNP_CMD.SNP_rec = 0x00;             //内容识别码：0
    SNP_CMD.SNP_len = 8+8;              //长度（包含首部）
    SNP_CMD.SNP_tran = 0x01;            //0001B,信息传递服务
    LOS_CAN_FirstPart(&SNP_CMD,FP_CMD);
    LOS_CAN_Transmit(LOS_CAN_NUM1, LOS_CAN_IDGen(ACK_NoNeed, 0XF0, nID_WheelX + FW_NUM, 0X00), FP_CMD, 8);

    if(angvel>8000)
    {
        angvel = 8000;  //角速度最大设置为8000rpm
    }

    if(angvel<-8000)
    {
        angvel = -8000;
    }

    FP_CMD[0]=0X02;
    FP_CMD[1]=nID_WheelX+FW_NUM;
    FP_CMD[2]=0XD2;
    memcpy(&FP_CMD[3],&angvel,4);
    FP_CMD[7]=FP_CMD[0]+FP_CMD[1]+FP_CMD[2]+FP_CMD[3]+FP_CMD[4]+FP_CMD[5]+FP_CMD[6];
    LOS_CAN_Transmit(LOS_CAN_NUM1, LOS_CAN_IDGen(ACK_NoNeed, 0XF0, nID_WheelX + FW_NUM, 0XFF), FP_CMD, 8);
}

void LOS_FW_SetTorque(uint8_t FW_NUM, float torque)
{
    LOS_SNP_t SNP_CMD;                  //首部配置结构体
    uint8_t FP_CMD[8];                  //首部内容
    float tor = torque;
    SNP_CMD.SNP_dest = nID_WheelX + FW_NUM;     //飞轮XYZS
    SNP_CMD.SNP_source = 0xF0;          //OBC发送
    SNP_CMD.SNP_rec = 0x00;             //内容识别码：0
    SNP_CMD.SNP_len = 8+8;              //长度（包含首部）
    SNP_CMD.SNP_tran = 0x01;            //0001B,信息传递服务
    LOS_CAN_FirstPart(&SNP_CMD,FP_CMD);
    LOS_CAN_Transmit(LOS_CAN_NUM1, LOS_CAN_IDGen(ACK_NoNeed, 0XF0, nID_WheelX + FW_NUM, 0X00), FP_CMD, 8);

    // if(torque>2.5)
    // {
    //     torque = 2.5;  //力矩最大设置为2.5mNm
    // }

    // if(torque<-2.5)
    // {
    //     torque = -2.5;
    // }

    FP_CMD[0]=0X03;
    FP_CMD[1]=nID_WheelX+FW_NUM;
    FP_CMD[2]=0XD3;
    memcpy(&FP_CMD[3], &tor, 4);
    FP_CMD[7]=FP_CMD[0]+FP_CMD[1]+FP_CMD[2]+FP_CMD[3]+FP_CMD[4]+FP_CMD[5]+FP_CMD[6];
    LOS_CAN_Transmit(LOS_CAN_NUM1, LOS_CAN_IDGen(ACK_NoNeed, 0XF0, nID_WheelX + FW_NUM, 0XFF), FP_CMD, 8);
}

void LOS_FW_PWROn(uint8_t FW_NUM)
{
    LOS_EPS_Switch(13 + FW_NUM, SWITCH_ON);
    STA_FW[FW_NUM].CMD = 1;
}

void LOS_FW_PWROff(uint8_t FW_NUM)
{
    LOS_EPS_Switch(13 + FW_NUM, SWITCH_OFF);
    STA_FW[FW_NUM].CMD = 0;
    STA_FW[FW_NUM].MSG = 1;
}


void LOS_CAN_FWXCallBack(LOS_CANMSG_t* can_re)
{
    //首部校验用法：首部校验通过/未通过会改变canerrflag的值，所有消息会被先存在缓冲区，canerrflag有效时才会转存，无效时舍弃
    uint8_t id_filter[4];
    uint8_t sum = 0;                       //首部校验
    uint8_t sum_check = 0;                 //首部校验
    static uint8_t canerrflag = 0;         //表示首部校验是否通过，未通过：1，通过：0
    static uint8_t angvel_buf[4];          //角速度断帧缓存
    uint8_t tm_check = 0;                  //遥测返回校验
    float fbuf;

    memcpy(id_filter, &(can_re->ID), 4);   //将帧ID分为4个字节存储，便于处理
    if (id_filter[3]==0x00)                //若为首帧
    {
        sum = can_re->msg[0] + can_re->msg[1] + can_re->msg[2] + can_re->msg[3] + can_re->msg[4] + can_re->msg[5] + can_re->msg[6];
        sum_check = 0x100 - sum;
        if (sum_check == can_re->msg[7])   //校验和检查，消息有效性判断
        {
            canerrflag = 0;
            STA_FW[FW_X].VAL = 0;
        }
        else 
        {
           canerrflag = 1;
           STA_FW[FW_X].VAL = 1; 
        }

    }
    else if (id_filter[3]==0xFF)  //若为尾帧
    {
        if (canerrflag)
        {
            LOS_CAN_DoNothing();
        }
        else 
        {
            angvel_buf[1] = can_re->msg[0]; //转速反馈第二字节
            angvel_buf[2] = can_re->msg[1]; //转速反馈第三字节
            angvel_buf[3] = can_re->msg[2]; //转速反馈第四字节
            memcpy(&fbuf,angvel_buf,4); //飞轮转速
//            if (isfinite(fbuf) !=0 )    //浮点数非数判断
//            {
//                STA_FW[FW_X].TM_RPM = fbuf;
//            }
//            else
//            {
//                STA_FW[FW_X].VAL = 1;
//            }
            memcpy(&fbuf,&can_re->msg[3],4); //电流
//            if (isfinite(fbuf) !=0 )    //浮点数非数判断
//            {
//                STA_FW[FW_X].TM_CRT = fbuf;
//            }
//            else
//            {
//                STA_FW[FW_X].VAL = 1;     //无效
//            }
            tm_check = tm_check + can_re->msg[0] + can_re->msg[1] + can_re->msg[2] + can_re->msg[3] + can_re->msg[4] + can_re->msg[5] + can_re->msg[6];
            if (tm_check == can_re->msg[7])
            {

            }
            else
            {
                STA_FW[FW_X].VAL = 1;
                STA_FW[FW_X].TMCNT++;  //遥测计数加一
            }

            memcpy(&TMPACK_FWX[18], &can_re->msg[0], 8);

            STA_FW[FW_X].HL_TXF = 0;
        }
        canerrflag = 0;   //最后一帧时清除标志位
        
    }
    else   //若为中间帧
    {
        if (canerrflag)
        {
            LOS_CAN_DoNothing();
        }
        else 
        {
            if (can_re->msg[0]==0x1D)  //判断为遥测返回
            {
                    STA_FW[FW_X].TM_CNT=can_re->msg[1];  //遥测计数器
                    STA_FW[FW_X].TM_MOD=can_re->msg[2];  //飞轮模式
                    memcpy(&STA_FW[FW_X].TM_LSET, &can_re->msg[3], 4);  //最后一次设置值
                    angvel_buf[0] = can_re->msg[7]; //转速反馈首字节
                    tm_check = can_re->msg[0] + can_re->msg[1] + can_re->msg[2] + can_re->msg[3] + can_re->msg[4] + can_re->msg[5] + can_re->msg[6] + can_re->msg[7];
                    memcpy(&TMPACK_FWX[0], &LOS_Time_SecFromBoot, 4);
                    memcpy(&TMPACK_FWX[4], &LOS_Time_MilSec, 2);
                    memcpy(&TMPACK_FWX[6], &LOS_Time_Sec, 4);
                    memcpy(&TMPACK_FWX[10], &can_re->msg[0], 8);
            }
            else  //一眼假，鉴定为接收错误
            {
                LOS_CAN_DoNothing();
            }
        }
    }
}

void LOS_CAN_FWYCallBack(LOS_CANMSG_t* can_re)
{
    //首部校验用法：首部校验通过/未通过会改变canerrflag的值，所有消息会被先存在缓冲区，canerrflag有效时才会转存，无效时舍弃
    uint8_t id_filter[4];
    uint8_t sum = 0;                       //首部校验
    uint8_t sum_check = 0;                 //首部校验
    static uint8_t canerrflag = 0;         //表示首部校验是否通过，未通过：1，通过：0
    static uint8_t angvel_buf[4];          //角速度断帧缓存
    uint8_t tm_check = 0;                  //遥测返回校验
    float fbuf;

    memcpy(id_filter, &(can_re->ID), 4);   //将帧ID分为4个字节存储，便于处理
    if (id_filter[3]==0x00)                //若为首帧
    {
        sum = can_re->msg[0] + can_re->msg[1] + can_re->msg[2] + can_re->msg[3] + can_re->msg[4] + can_re->msg[5] + can_re->msg[6];
        sum_check = 0x100 - sum;
        if (sum_check == can_re->msg[7])   //校验和检查，消息有效性判断
        {
            canerrflag = 0;
            STA_FW[FW_Y].VAL = 0;
        }
        else 
        {
           canerrflag = 1;
           STA_FW[FW_Y].VAL = 1; 
        }

    }
    else if (id_filter[3]==0xFF)  //若为尾帧
    {
        if (canerrflag)
        {
            LOS_CAN_DoNothing();
        }
        else 
        {
            angvel_buf[1] = can_re->msg[0]; //转速反馈第二字节
            angvel_buf[2] = can_re->msg[1]; //转速反馈第三字节
            angvel_buf[3] = can_re->msg[2]; //转速反馈第四字节
            memcpy(&fbuf,angvel_buf,4); //飞轮转速
//            if (isfinite(fbuf) !=0 )    //浮点数非数判断
//            {
//                STA_FW[FW_Y].TM_RPM = fbuf;
//            }
//            else
//            {
//                STA_FW[FW_Y].VAL = 1;
//            }
            memcpy(&fbuf,&can_re->msg[3],4); //电流
//            if (isfinite(fbuf) !=0 )    //浮点数非数判断
//            {
//                STA_FW[FW_Y].TM_CRT = fbuf;
//            }
//            else
//            {
//                STA_FW[FW_Y].VAL = 1;     //无效
//            }
            tm_check = tm_check + can_re->msg[0] + can_re->msg[1] + can_re->msg[2] + can_re->msg[3] + can_re->msg[4] + can_re->msg[5] + can_re->msg[6];
            if (tm_check == can_re->msg[7])
            {

            }
            else
            {
                STA_FW[FW_Y].VAL = 1;
                STA_FW[FW_Y].TMCNT++;  //遥测计数加一
            }

            memcpy(&TMPACK_FWY[18], &can_re->msg[0], 8);

            STA_FW[FW_Y].HL_TXF = 0;
        }
        canerrflag = 0;   //最后一帧时清除标志位
        
    }
    else   //若为中间帧
    {
        if (canerrflag)
        {
            LOS_CAN_DoNothing();
        }
        else 
        {
            if (can_re->msg[0]==0x1D)  //判断为遥测返回
            {
                    STA_FW[FW_Y].TM_CNT=can_re->msg[1];  //遥测计数器
                    STA_FW[FW_Y].TM_MOD=can_re->msg[2];  //飞轮模式
                    memcpy(&STA_FW[FW_Y].TM_LSET, &can_re->msg[3], 4);  //最后一次设置值
                    angvel_buf[0] = can_re->msg[7]; //转速反馈首字节
                    tm_check = can_re->msg[0] + can_re->msg[1] + can_re->msg[2] + can_re->msg[3] + can_re->msg[4] + can_re->msg[5] + can_re->msg[6] + can_re->msg[7];
                    memcpy(&TMPACK_FWY[0], &LOS_Time_SecFromBoot, 4);
                    memcpy(&TMPACK_FWY[4], &LOS_Time_MilSec, 2);
                    memcpy(&TMPACK_FWY[6], &LOS_Time_Sec, 4);
                    memcpy(&TMPACK_FWY[10], &can_re->msg[0], 8);
            }
            else  //一眼假，鉴定为接收错误
            {
                LOS_CAN_DoNothing();
            }
        }
    }
}

void LOS_CAN_FWZCallBack(LOS_CANMSG_t* can_re)
{
    //首部校验用法：首部校验通过/未通过会改变canerrflag的值，所有消息会被先存在缓冲区，canerrflag有效时才会转存，无效时舍弃
    uint8_t id_filter[4];
    uint8_t sum = 0;                       //首部校验
    uint8_t sum_check = 0;                 //首部校验
    static uint8_t canerrflag = 0;         //表示首部校验是否通过，未通过：1，通过：0
    static uint8_t angvel_buf[4];          //角速度断帧缓存
    uint8_t tm_check = 0;                  //遥测返回校验
    float fbuf;

    memcpy(id_filter, &(can_re->ID), 4);   //将帧ID分为4个字节存储，便于处理
    if (id_filter[3]==0x00)                //若为首帧
    {
        sum = can_re->msg[0] + can_re->msg[1] + can_re->msg[2] + can_re->msg[3] + can_re->msg[4] + can_re->msg[5] + can_re->msg[6];
        sum_check = 0x100 - sum;
        if (sum_check == can_re->msg[7])   //校验和检查，消息有效性判断
        {
            canerrflag = 0;
            STA_FW[FW_Z].VAL = 0;
        }
        else 
        {
           canerrflag = 1;
           STA_FW[FW_Z].VAL = 1; 
        }

    }
    else if (id_filter[3]==0xFF)  //若为尾帧
    {
        if (canerrflag)
        {
            LOS_CAN_DoNothing();
        }
        else 
        {
            angvel_buf[1] = can_re->msg[0]; //转速反馈第二字节
            angvel_buf[2] = can_re->msg[1]; //转速反馈第三字节
            angvel_buf[3] = can_re->msg[2]; //转速反馈第四字节
            memcpy(&fbuf,angvel_buf,4); //飞轮转速
//            if (isfinite(fbuf) !=0 )    //浮点数非数判断
//            {
//                STA_FW[FW_Z].TM_RPM = fbuf;
//            }
//            else
//            {
//                STA_FW[FW_Z].VAL = 1;
//            }
            memcpy(&fbuf,&can_re->msg[3],4); //电流
//            if (isfinite(fbuf) !=0 )    //浮点数非数判断
//            {
//                STA_FW[FW_Z].TM_CRT = fbuf;
//            }
//            else
//            {
//                STA_FW[FW_Z].VAL = 1;     //无效
//            }
            tm_check = tm_check + can_re->msg[0] + can_re->msg[1] + can_re->msg[2] + can_re->msg[3] + can_re->msg[4] + can_re->msg[5] + can_re->msg[6];
            if (tm_check == can_re->msg[7])
            {

            }
            else
            {
                STA_FW[FW_Z].VAL = 1;
                STA_FW[FW_Z].TMCNT++;  //遥测计数加一
            }

            memcpy(&TMPACK_FWZ[18], &can_re->msg[0], 8);

            STA_FW[FW_Z].HL_TXF = 0;
        }
        canerrflag = 0;   //最后一帧时清除标志位
        
    }
    else   //若为中间帧
    {
        if (canerrflag)
        {
            LOS_CAN_DoNothing();
        }
        else 
        {
            if (can_re->msg[0]==0x1D)  //判断为遥测返回
            {
                    STA_FW[FW_Z].TM_CNT=can_re->msg[1];  //遥测计数器
                    STA_FW[FW_Z].TM_MOD=can_re->msg[2];  //飞轮模式
                    memcpy(&STA_FW[FW_Z].TM_LSET, &can_re->msg[3], 4);  //最后一次设置值
                    angvel_buf[0] = can_re->msg[7]; //转速反馈首字节
                    tm_check = can_re->msg[0] + can_re->msg[1] + can_re->msg[2] + can_re->msg[3] + can_re->msg[4] + can_re->msg[5] + can_re->msg[6] + can_re->msg[7];
                    memcpy(&TMPACK_FWZ[0], &LOS_Time_SecFromBoot, 4);
                    memcpy(&TMPACK_FWZ[4], &LOS_Time_MilSec, 2);
                    memcpy(&TMPACK_FWZ[6], &LOS_Time_Sec, 4);
                    memcpy(&TMPACK_FWZ[10], &can_re->msg[0], 8);
            }
            else  //一眼假，鉴定为接收错误
            {
                LOS_CAN_DoNothing();
            }
        }
    }
}

void LOS_CAN_FWSCallBack(LOS_CANMSG_t* can_re)
{
    //首部校验用法：首部校验通过/未通过会改变canerrflag的值，所有消息会被先存在缓冲区，canerrflag有效时才会转存，无效时舍弃
    uint8_t id_filter[4];
    uint8_t sum = 0;                       //首部校验
    uint8_t sum_check = 0;                 //首部校验
    static uint8_t canerrflag = 0;         //表示首部校验是否通过，未通过：1，通过：0
    static uint8_t angvel_buf[4];          //角速度断帧缓存
    uint8_t tm_check = 0;                  //遥测返回校验
    float fbuf;

    memcpy(id_filter, &(can_re->ID), 4);   //将帧ID分为4个字节存储，便于处理
    if (id_filter[3]==0x00)                //若为首帧
    {
        sum = can_re->msg[0] + can_re->msg[1] + can_re->msg[2] + can_re->msg[3] + can_re->msg[4] + can_re->msg[5] + can_re->msg[6];
        sum_check = 0x100 - sum;
        if (sum_check == can_re->msg[7])   //校验和检查，消息有效性判断
        {
            canerrflag = 0;
            STA_FW[FW_S].VAL = 0;
        }
        else 
        {
           canerrflag = 1;
           STA_FW[FW_S].VAL = 1; 
        }

    }
    else if (id_filter[3]==0xFF)  //若为尾帧
    {
        if (canerrflag)
        {
            LOS_CAN_DoNothing();
        }
        else 
        {
            angvel_buf[1] = can_re->msg[0]; //转速反馈第二字节
            angvel_buf[2] = can_re->msg[1]; //转速反馈第三字节
            angvel_buf[3] = can_re->msg[2]; //转速反馈第四字节
            memcpy(&fbuf,angvel_buf,4); //飞轮转速
//            if (isfinite(fbuf) !=0 )    //浮点数非数判断
//            {
//                STA_FW[FW_S].TM_RPM = fbuf;
//            }
//            else
//            {
//                STA_FW[FW_S].VAL = 1;
//            }
            memcpy(&fbuf,&can_re->msg[3],4); //电流
//            if (isfinite(fbuf) !=0 )    //浮点数非数判断
//            {
//                STA_FW[FW_S].TM_CRT = fbuf;
//            }
//            else
//            {
//                STA_FW[FW_S].VAL = 1;     //无效
//            }
//            tm_check = tm_check + can_re->msg[0] + can_re->msg[1] + can_re->msg[2] + can_re->msg[3] + can_re->msg[4] + can_re->msg[5] + can_re->msg[6];
            if (tm_check == can_re->msg[7])
            {

            }
            else
            {
                STA_FW[FW_S].VAL = 1;
                STA_FW[FW_S].TMCNT++;  //遥测计数加一
            }

            memcpy(&TMPACK_FWS[18], &can_re->msg[0], 8);

            STA_FW[FW_S].HL_TXF = 0;
        }
        canerrflag = 0;   //最后一帧时清除标志位
        
    }
    else   //若为中间帧
    {
        if (canerrflag)
        {
            LOS_CAN_DoNothing();
        }
        else 
        {
            if (can_re->msg[0]==0x1D)  //判断为遥测返回
            {
                    STA_FW[FW_S].TM_CNT=can_re->msg[1];  //遥测计数器
                    STA_FW[FW_S].TM_MOD=can_re->msg[2];  //飞轮模式
                    memcpy(&STA_FW[FW_S].TM_LSET, &can_re->msg[3], 4);  //最后一次设置值
                    angvel_buf[0] = can_re->msg[7]; //转速反馈首字节
                    tm_check = can_re->msg[0] + can_re->msg[1] + can_re->msg[2] + can_re->msg[3] + can_re->msg[4] + can_re->msg[5] + can_re->msg[6] + can_re->msg[7];
                    memcpy(&TMPACK_FWS[0], &LOS_Time_SecFromBoot, 4);
                    memcpy(&TMPACK_FWS[4], &LOS_Time_MilSec, 2);
                    memcpy(&TMPACK_FWS[6], &LOS_Time_Sec, 4);
                    memcpy(&TMPACK_FWS[10], &can_re->msg[0], 8);
            }
            else  //一眼假，鉴定为接收错误
            {
                LOS_CAN_DoNothing();
            }
        }
    }
}


void LOS_CAN_FWCallBack(uint8_t FW_NUM, LOS_CANMSG_t* can_re)
{
    //首部校验用法：首部校验通过/未通过会改变canerrflag的值，所有消息会被先存在缓冲区，canerrflag有效时才会转存，无效时舍弃
    uint8_t id_filter[4];
    uint8_t sum = 0;                       //首部校验
    uint8_t sum_check = 0;                 //首部校验
    static uint8_t canerrflag = 0;         //表示首部校验是否通过，未通过：1，通过：0
    static uint8_t angvel_buf[4];          //角速度断帧缓存
    uint8_t tm_check = 0;                  //遥测返回校验
    float fbuf;

    memcpy(id_filter, &(can_re->ID), 4);   //将帧ID分为4个字节存储，便于处理
    if (id_filter[3]==0x00)                //若为首帧
    {
        sum = can_re->msg[0] + can_re->msg[1] + can_re->msg[2] + can_re->msg[3] + can_re->msg[4] + can_re->msg[5] + can_re->msg[6];
        sum_check = 0x100 - sum;
        if (sum_check == can_re->msg[7])   //校验和检查，消息有效性判断
        {
            canerrflag = 0;
            STA_FW[FW_NUM].VAL = 0;
        }
        else 
        {
           canerrflag = 1;
           STA_FW[FW_NUM].VAL = 1; 
        }

    }
    else if (id_filter[3]==0xFF)  //若为尾帧
    {
        if (canerrflag)
        {
            LOS_CAN_DoNothing();
        }
        else 
        {
            angvel_buf[1] = can_re->msg[0]; //转速反馈第二字节
            angvel_buf[2] = can_re->msg[1]; //转速反馈第三字节
            angvel_buf[3] = can_re->msg[2]; //转速反馈第四字节
            memcpy(&fbuf,angvel_buf,4); //飞轮转速
//            if (isfinite(fbuf) !=0 )    //浮点数非数判断
//            {
//                STA_FW[FW_NUM].TM_RPM = fbuf;
//            }
//            else
//            {
//                STA_FW[FW_NUM].VAL = 1;
//            }
            memcpy(&fbuf,&can_re->msg[3],4); //电流
//            if (isfinite(fbuf) !=0 )    //浮点数非数判断
//            {
//                STA_FW[FW_NUM].TM_CRT = fbuf;
//            }
//            else
//            {
//                STA_FW[FW_NUM].VAL = 1;     //无效
//            }
            tm_check = tm_check + can_re->msg[0] + can_re->msg[1] + can_re->msg[2] + can_re->msg[3] + can_re->msg[4] + can_re->msg[5] + can_re->msg[6];
            if (tm_check == can_re->msg[7])
            {

            }
            else
            {
                STA_FW[FW_NUM].VAL = 1;
                STA_FW[FW_NUM].TMCNT++;  //遥测计数加一
            }

            switch(FW_NUM)
            {
                case FW_X:
                memcpy(&TMPACK_FWX[18], &can_re->msg[0], 8);
                break;

                case FW_Y:
                memcpy(&TMPACK_FWY[18], &can_re->msg[0], 8);
                break;

                case FW_Z:
                memcpy(&TMPACK_FWZ[18], &can_re->msg[0], 8);
                break;

                case FW_S:
                memcpy(&TMPACK_FWS[18], &can_re->msg[0], 8);
                break;

                default:
                break;
            }

            STA_FW[FW_NUM].HL_TXF = 0;
        }
        canerrflag = 0;   //最后一帧时清除标志位
        
    }
    else   //若为中间帧
    {
        if (canerrflag)
        {
            LOS_CAN_DoNothing();
        }
        else 
        {
            if (can_re->msg[0]==0x1D)  //判断为遥测返回
            {
                    STA_FW[FW_NUM].TM_CNT=can_re->msg[1];  //遥测计数器
                    STA_FW[FW_NUM].TM_MOD=can_re->msg[2];  //飞轮模式
                    memcpy(&STA_FW[FW_NUM].TM_LSET, &can_re->msg[3], 4);  //最后一次设置值
                    angvel_buf[0] = can_re->msg[7]; //转速反馈首字节
                    tm_check = can_re->msg[0] + can_re->msg[1] + can_re->msg[2] + can_re->msg[3] + can_re->msg[4] + can_re->msg[5] + can_re->msg[6] + can_re->msg[7];
                    switch(FW_NUM)
                    {
                        case FW_X:
                        memcpy(&TMPACK_FWX[0], &LOS_Time_SecFromBoot, 4);
                        memcpy(&TMPACK_FWX[4], &LOS_Time_MilSec, 2);
                        memcpy(&TMPACK_FWX[6], &LOS_Time_Sec, 4);
                        memcpy(&TMPACK_FWX[10], &can_re->msg[0], 8);
                        break;

                        case FW_Y:
                        memcpy(&TMPACK_FWY[0], &LOS_Time_SecFromBoot, 4);
                        memcpy(&TMPACK_FWY[4], &LOS_Time_MilSec, 2);
                        memcpy(&TMPACK_FWY[6], &LOS_Time_Sec, 4);
                        memcpy(&TMPACK_FWY[10], &can_re->msg[0], 8);
                        break;

                        case FW_Z:
                        memcpy(&TMPACK_FWZ[0], &LOS_Time_SecFromBoot, 4);
                        memcpy(&TMPACK_FWZ[4], &LOS_Time_MilSec, 2);
                        memcpy(&TMPACK_FWZ[6], &LOS_Time_Sec, 4);
                        memcpy(&TMPACK_FWZ[10], &can_re->msg[0], 8);
                        break;

                        case FW_S:
                        memcpy(&TMPACK_FWS[0], &LOS_Time_SecFromBoot, 4);
                        memcpy(&TMPACK_FWS[4], &LOS_Time_MilSec, 2);
                        memcpy(&TMPACK_FWS[6], &LOS_Time_Sec, 4);
                        memcpy(&TMPACK_FWS[10], &can_re->msg[0], 8);
                        break;

                        default:
                        break;
                    }
            }
            else  //一眼假，鉴定为接收错误
            {
                LOS_CAN_DoNothing();
            }
        }
    }
}
